#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Ultrasonic,     sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     MiddleMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     RightMotor,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

int BeaconPost = 0;

void FindBeaconPost()
{
	if (SensorValue[IR1200] == 5)
	{
		BeaconPost = 2;
	}
	else if (SensorValue[IR1200] > 5)
	{
		BeaconPost = 3;
	}
	else if (SensorValue[IR1200] < 5)
	{
		BeaconPost = 1;
	}
	else
	{
		BeaconPost = 0;
	}
}

void GoToBeacon()
{
	if (BeaconPost == 1)
	{
		turnL(30,500);
		Forward(30,1000);
		turnR(30,1000);
		// Here the servo programming will be inserted.
	}
	else if(BeaconPost == 2)
	{

	}
	else if(BeaconPost == 3)
	{
		turnR(30,500);
		Forward(30,1000);
		turnL(30,1000);
		// Here the servo programming will be inserted.
	}
}

task main()
{
	turnR(30,500);
	Forward(30,1000);
	turnL(30,1000);
	FindBeaconPost();
	GoToBeacon();
}
